/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldmodel;

import java.util.SortedSet;
import java.util.TreeSet;
import java.util.Vector;

import robocup.component.DAInamiteConstants;
import robocup.component.PConf;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.ControllerNumbers.REFEREE_MESSAGE;
import robocup.component.actions.Action;
import robocup.component.actions.CatchAction;
import robocup.component.actions.DashAction;
import robocup.component.actions.MoveAction;
import robocup.component.actions.TurnAction;
import robocup.component.filters.MCLFilter;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.FlagInfo;
import robocup.component.infotypes.LineInfo;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.worldmodel.particlefilter.ParticleFilter;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.Flag;
import robocup.component.worldobjects.Line;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.StaticObject;
import robocup.component.worldobjects.Player.RELATIVE;

/**
 * MeModel is the part of the WorldModel which implements functions to 
 * calculate and store the actual state of the player itself (position, 
 * speed, direction etc.). It is updated each time an info arrives.
 */
public class MeModel implements IModel {

	/**
	 * Class PlayerState is used to return the position and head angle of the
	 * player from the <code>approxMyPos</code> methods.
	 */
	private final class PlayerState {

		private Vektor position = new Vektor();

		private double headAngle = 0;

		/**
		 * <code>PlayerState</code> constructor
		 * 
		 * @param playerPos - player position
		 * @param headAngle - player head angle (abs)
		 */
		public PlayerState(Vektor playerPos, double headAngle) {

			this.position = playerPos;
			this.headAngle = headAngle;
		}

		/**
		 * @return Returns the headAngle.
		 */
		public final double getHeadAngle() {

			return this.headAngle;
		}

		/**
		 * @return Returns the pos.
		 */
		public final Vektor getPosition() {

			return this.position;
		}
	}

	// the world for other worldmodel parts methods calls
	private WorldModel world;

	private SConf sConf = SConf.getInstance();

	// the player
	private Player me = Player.getDummyInstance();

	// the me of last cycle
	private Player lastMe = Player.getDummyInstance();

	// the me of as predicted for this cycle
  // based on the last world and the last action
	private Player predictedMe = Player.getDummyInstance();

	// the me after bs
	private Player tmpMe = Player.getDummyInstance();

	// the view width of the player in degree (91 for debugging ;-)
	private double viewWidth = 91;

  //particle filter for position estimation (high quality)
	private ParticleFilter particleFilter = new ParticleFilter();

  //particle filter for position estimation (fast but low quality)
	private MCLFilter mclFilter;

	/**
	 * <code>MeModel</code> constructor - a model containing all information
	 * about the self-agent.
	 * 
	 * @param world -
	 *            the current world-model (parent)
	 */
	public MeModel(WorldModel world) {

		this.mclFilter = new MCLFilter();
		this.world = world;
	}

	/**
	 * This method overwrites the me-player, created after receiving the
	 * init-command. This way all models are working on the same reference.
	 * 
	 * @param self -
	 *            the new me-reference
	 */
	public void initSelf(Player self) {

		this.me = self;
	}

	/**
	 * updateMeSense updates the player <code>me</code> from the SenseBodyInfo
	 * <code>sbInfo</code>. Some knowledge is explicitly given by the 
   * <code>sbInfo</code>. Other knowledge has to be estimated based on the
   * last wolrd state and the performed actions.
	 * 
	 * @param sbInfo -
	 *            Info for the update
	 */
	public void update(SenseBodyInfo sbInfo) {

		// save last me
		this.lastMe.copy(this.me);

		// set the expected me (used for collision model)
		if (sbInfo.getLastSentActions() != null) {
			this.predictedMe.copy(this.world.getSelfRef());
			for (Action a : sbInfo.getLastSentActions()) {
				this.predictedMe.predictThisAfterAction(a);
			}
		}

		// The following code calculates a prediction of the player's speed,
		// position, body and head angle based on the information of the
		// last cycle and the information contained in the sense body info.
		// View UvA master's thesis chapter 6.4.1 for further information.
		// Complain to till, if it does not work!
		double lastActionTurnAngle = 0.0;
		Vector<Action> lastSentActions;

		this.viewWidth = sbInfo.getViewWidth().getFactor()
				* this.sConf.VISIBLE_ANGLE;

		// set new body direction to old body direction
		double newBodyDir = this.me.getBodyDir();

		double stamina = this.me.getStamina();
		double recovery = this.me.getRecovery();

		// if actions were sent in the last cycle
		if ((lastSentActions = sbInfo.getLastSentActions()) != null) {
			// estimate the new player based on the last world and the
      //sent actions (see UvA Master thesis for details)
			for (Action lastAction : lastSentActions) {
				switch (lastAction.getType()) {
				case TURN:
					TurnAction tAction = (TurnAction) lastAction; // changed

					lastActionTurnAngle += tAction.getAngle();
					if (lastActionTurnAngle > this.sConf.MAXMOMENT) {
						lastActionTurnAngle = this.sConf.MAXMOMENT;
					} else if (lastActionTurnAngle < -this.sConf.MAXMOMENT) {
						lastActionTurnAngle = -this.sConf.MAXMOMENT;
					}
					// Note that equation 6.9 of the master's thesis is
					// incorrect.
					// The effective turn angle is calculated as follows:
					double speed = this.me.getSpeed().getLength();
					// equation 3.31 of master's thesis says: eff =
					// last/(1+intert*speed)
					// equation 6.9 of master's thesis says: eff =
					// last/(1+inert)*speed
					// equation 6.9 seems to be incorrect!
					double effectiveTurnAngle = 0.0;
					effectiveTurnAngle = lastActionTurnAngle
							/ (1.0 + this.me.getPConf().INERTIA_MOMENT * speed);
					newBodyDir += effectiveTurnAngle;
					newBodyDir = Vektor.normalize(newBodyDir);
					break;

				case CATCH_BALL:
					CatchAction cAction = (CatchAction) lastAction;
					newBodyDir += cAction.getDirection();
					newBodyDir = Vektor.normalize(newBodyDir);
					break;

				case MOVE:
					if (!SConf.getInstance().COACH) {
						// after a MOVE action, the actual pos and speed in WM
						// are not valid anymore
						MoveAction mAction = (MoveAction) lastAction;
						double posX = mAction.getX();
						double posY = -mAction.getY();
						Vektor pos = new Vektor(Vektor.XY, posX, posY);
						pos.rotate(-90);
						// reset the filter
						this.mclFilter.reset(pos);
						this.me.setCycle(sbInfo.getCycle());
						this.me.setPosition(pos);
						this.me.getSpeed().reset();
					}
					break;

				case DASH:
					double power = ((DashAction) lastAction).getPower();
					if (power > this.sConf.MAXPOWER) {
						power = this.sConf.MAXPOWER;
					} else if (power < this.sConf.MINPOWER) {
						power = this.sConf.MINPOWER;
					}

					// calculate stamina for calculating recovery according to
					// MT Alg. 3.1: stamina model algorithm

					// reduce stamina according to dash power
					if (power < 0) {
						stamina -= 2 * Math.abs(power);
					} else {
						stamina -= power;
					}
					break;

				case TACKLE:
					// tackle counter arrives directly in bs-info
					break;

				case KICK:
					if (PLAY_MODE.META_STANDARD_OWN.contains(this.world
							.getPreviousPlaymode())
							&& this.world.getLastPlayModeMessageCycle() > this.world
									.getBodyCycle() - 2) {
            //remember that this player did the standard and therefor
            //is not allow to intercept the ball!!
						this.me.setKickedStandard(true);
					} else {
						this.me.setKickedStandard(false);
					}

				default:
					;

				}
			} // end for
		} // end if

		// set new position to old position
		Vektor newPos = this.me.getPosition().cloned();

		final double speedDir = Vektor.normalize(newBodyDir
				+ sbInfo.getNeckAngle() + sbInfo.getSpeedDir());

		Vektor newSpeed = new Vektor(sbInfo.getSpeed(), speedDir);

		if (this.me.getPConf() == null) {
			this.me.setPConf(new PConf());
		}

		if (newSpeed.getLength() > this.me.getPConf().PLAYER_SPEED_MAX
				* this.me.getPConf().PLAYER_DECAY) {
			newSpeed.setLength(this.me.getPConf().PLAYER_SPEED_MAX
					* this.me.getPConf().PLAYER_DECAY);
		}

		Vektor oldSpeed = newSpeed.cloned();
		oldSpeed.mult(1.0 / this.me.getPConf().PLAYER_DECAY);

		// new position depends on the old speed
		newPos.addToThis(oldSpeed);

		// send odometry data to the filter
		this.mclFilter.setU(oldSpeed);

		// BEGIN calculate recovery according to MT Alg. 3.1: stamina model
		// algorithm reduce recovery when stamina is below recovery decrement 
    // threshold
		if (stamina <= this.sConf.RECOVER_DEC_THR * this.sConf.STAMINA_MAX) {
			if (recovery > this.sConf.RECOVER_MIN) {
				recovery -= this.sConf.RECOVER_DEC;
			}
			recovery = Math.max(this.sConf.RECOVER_MIN, recovery);
		}
		// algorithm

		this.particleFilter.translatePosition(oldSpeed);

		this.me.setCycle(sbInfo.getCycle());
		this.me.setPosition(newPos);
		this.me.setSpeed(newSpeed);
		this.me.setBodyDir(newBodyDir);
		this.me.setNeckAngle(sbInfo.getNeckAngle());
		this.me.setStamina(sbInfo.getStamina());
		this.me.setEffort(sbInfo.getEffort());
		this.me.setRecovery(recovery);
		// point at status, absolute angle
		this.me.setArmDirection(Vektor.normalize(sbInfo.getArmDir()
				+ (newBodyDir + sbInfo.getNeckAngle())));
		this.me.setArmDistance(sbInfo.getArmDist());
		this.me.setArmExpires(sbInfo.getArmExpires());
		this.me.setArmMovable(sbInfo.getArmMoveable() == 0);
		this.me.setTackledeadCountdown(sbInfo.getTackleExpires());

		this.tmpMe.copy(this.me);
	}

	/**
	 * <code>updateVisual</code> updates the player <code>me</code> from the
	 * VisualInfo <code>vInfo</code>. Of special importance here is the update
   * of the players head direction and its position as these data has
   * a huge impact on most of the world knowledge. Because of that the MeModel
   * is the 1st model to be updated after an VI arrival.
   * 
   * There exist many sophisticated methods for calculating a player's 
   * position. This method uses 2 algorithms. The first simply takes some near
   * flags. The 2nd is more complicated and uses a particle filter similar
   * to the one described in the UvA master's thesis (see manual). As the
   * position quality is of less importance with no ball near we use the
   * 1st and faster method for distant balls and the 2nd for near ones. 
   * 
	 * @param vInfo - new visual info
	 */
	public void update(VisualInfo vInfo) {

		// tests shown, that only one line can be seen pro cycle
		LineInfo[] lines = vInfo.getLines();
		LineInfo line = null;
		if (lines.length > 0) {
			line = lines[0];
		}

		FlagInfo[] flags = vInfo.getFlags();
		/*
		 * We need only WM_MAX_FLAGS nearest flags, so we put them in a TreeSet.
		 * A TreeSet guarantees that its iterator will traverse the set in
		 * ascending element order. This is possible due to the fact, that all
		 * VisualInfoObjects implement java.lang.Comparable.
		 */
		SortedSet<FlagInfo> sortedFlags = new TreeSet<FlagInfo>();
		for (int i = 0; i < flags.length; i++) {
			if (flags[i] != null) {
				sortedFlags.add(flags[i]);
			}
		}

		int size = sortedFlags.size();
		if (size > 1) {
			// copy the sorted Flags into an array
			flags = new FlagInfo[size];
			int n = 0;
			for (FlagInfo fi : sortedFlags) {
				flags[n] = fi;
				n++;
			}

			Vector<Vektor> flagsPositions = new Vector<Vektor>();
			Vector<Double> flagsHeadDirs = new Vector<Double>();
			PlayerState pState = null;
			/*
			 * calculate our estimated position using a slightly modified
			 * version of the MT algorithm
			 */
			for (int i = 0, count = 0; i < size - 1
					&& count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
				for (int j = i + 1; j < size
						&& count < DAInamiteConstants.WM_MAX_FLAGS; j++) {
					pState = approxMyPos(flags[i], flags[j]);
					if (pState != null) {
						count++;
						flagsPositions.add(pState.getPosition());
						flagsHeadDirs.add(new Double(pState.getHeadAngle()));
					}
				}
			}

			// Vektor temp = averagePosArth(flagsPositions);
			Vektor temp = averagePosStdDev(flagsPositions);
			// Vektor temp = averagePosRMS(flagsPositions);
			Vektor newPos = this.me.getPosition().cloned();

			double headAngle;
			// now let's calculate the position using flags & line
			if (line != null && temp != null) {
				/*
				 * We know approximately where we are (newPos). This enables us
				 * an exact calculation of our headangle from the seen line.
				 */
				headAngle = getHAFromLine(line, newPos);
				/*
				 * Now we know our exact headangle. Thanks to this information
				 * we can calculate the Vektors from our position to the flags,
				 * because we have the distance to each flag and the angle,
				 * realtive to our own headangle. Than we substract these
				 * Vektors from the positions of the flags. This way we get one
				 * approximated position of the player for each flag we see. The
				 * last step is to calculate the average position from a Vector
				 * of all approximated positions We use only WM_MAX_FLAGS for
				 * the position calculation
				 */

				if (this.world.getDistance(this.world.getBallRef()) < 15) {
					// this.particleFilter.resetPosition(null);
					for (int i = 0, count = 0; i < size
							&& count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
						// Vektor from the player to the flag
						Vektor toFlag = new Vektor(flags[i].getDistance(),
								headAngle + flags[i].getDirection());
						// We need to handle the "felt" flags that have no IDs
						// (-1)
						Vektor flagpos = null;

						if (flags[i].getId() == -1) {
							/*
							 * There is a chance that we can find out, which
							 * flag we "feel". The method SConf.getFlag(Vektor
							 * position) returns a flag if the position lies
							 * near it. Near means in a circle around the flag
							 * (radius of the circle is
							 * DAInamiteConstants.WM_FLAG_RADIUS)
							 */
							Flag f = this.sConf.getFlag(newPos.add(toFlag));
							if (f != null) {
								// a flag has been found
								flagpos = f.getPosition();
							}
						} else {
							// we can get the flag directly by its ID
							flagpos = this.sConf.getFlag(flags[i].getId())
									.getPosition();
						}
						this.particleFilter.addFlag(flags[i], headAngle,
								flagpos);

					}
					temp = this.particleFilter.getPosition();
					if (temp != null && !Double.isNaN(temp.x)
							&& !Double.isNaN(temp.y)) {
						newPos = temp;
						double anglesin = 0;
						double anglecos = 0;
						long flagcount = 0;

						for (int i = 0, count = 0; i < size
								&& count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
							// Vektor from the player to the flag
							Vektor toFlag = new Vektor(flags[i].getDistance(),
									headAngle + flags[i].getDirection());
							// We need to handle the "felt" flags that have no
							// IDs (-1)
							Vektor flagpos = null;

							if (flags[i].getId() == -1) {
								/*
								 * There is a chance that we can find out, which
								 * flag we "feel". The method
								 * SConf.getFlag(Vektor position) returns a flag
								 * if the position lies near it. Near means in a
								 * circle around the flag (radius of the circle
								 * is DAInamiteConstants.WM_FLAG_RADIUS)
								 */
								Flag f = this.sConf.getFlag(newPos.add(toFlag));

								if (f != null) {
									// a flag has been found
									flagpos = f.getPosition();
								}
							} else {
								// we can get the flag directly by its ID
								flagpos = this.sConf.getFlag(flags[i].getId())
										.getPosition();
							}

							double x = -temp.x + flagpos.x + Double.MIN_VALUE;
							double y = -temp.y + flagpos.y + Double.MIN_VALUE;
							double angle = (Math.atan2(x, y)) - Math.PI
									* flags[i].getDirection() / 180.0;
							anglesin += Math.sin(angle) + Double.MIN_VALUE;
							anglecos += Math.cos(angle) + Double.MIN_VALUE;
							flagcount++;
						}

						// anglesin /= (double)flagcount;
						// anglecos /= (double)flagcount;

						headAngle = 180.0 * Math.atan2(anglesin, anglecos)
								/ Math.PI - 0.4;
					}
				} else {
					for (int i = 0, count = 0; i < size
							&& count < DAInamiteConstants.WM_MAX_FLAGS; i++) {
						// Vektor from the player to the flag
						Vektor toFlag = new Vektor(flags[i].getDistance(),
								headAngle + flags[i].getDirection());
						// We need to handle the "felt" flags that have no IDs
						// (-1)
						if (flags[i].getId() == -1) {
							/*
							 * There is a chance that we can find out, which
							 * flag we "feel". The method SConf.getFlag(Vektor
							 * position) returns a flag if the position lies
							 * near it. Near means in a circle around the flag
							 * (radius of the circle is
							 * DAInamiteConstants.WM_FLAG_RADIUS)
							 */
							Flag f = this.sConf.getFlag(newPos.add(toFlag));
							if (f != null) {
								// a flag has been found
								// flagsPositions.add(f.getPosition().sub(toFlag));
								this.mclFilter.sampleParticle(f.getPosition()
										.sub(toFlag), flags[i].getDistance());
								count++;
							}
						} else {
							// we can get the flag directly by its ID
							Vektor flag = this.sConf.getFlag(flags[i].getId())
									.getPosition();
							this.mclFilter.sampleParticle(flag.sub(toFlag),
									flags[i].getDistance());
							// flagsPositions.add(flag.sub(toFlag));
							count++;
						}
					}

					// average position
					// newPos = averagePosStdDev(flagsPositions);
					temp = this.mclFilter.getPosition();
					if (temp != null && !Double.isNaN(temp.x)
							&& !Double.isNaN(temp.y)) {
						newPos = temp;
						// System.out.println("[MeModel] newPos: "+newPos);
					}
					// newPos = averagePosArth(flagsPositions);
				}
			} else {
				/*
				 * The player didn't see any line, so we use the unprecise
				 * calculation from MT for the position. We also have to
				 * approximate the average headangle.
				 */
				// headAngle = averageHAArth(flagsHeadDirs);
				// rw for debugging System.out.println("No Line Seen");
				headAngle = averageHAStdDev(flagsHeadDirs);
			}
			// safety first ;)
			if (Double.isNaN(headAngle)) {
				headAngle = 0;
				this.me.setBodyDir(0.0);
				this.me.setNeckAngle(0.0);
				this.me.getPosition().reset();
				this.me.getSpeed().reset();
				this.me.getLastSeenPosition().reset();
				this.lastMe.setBodyDir(0.0);
				this.lastMe.setNeckAngle(0.0);
				this.lastMe.getPosition().reset();
				this.lastMe.getSpeed().reset();
				this.lastMe.getLastSeenPosition().reset();
				this.mclFilter.reset(this.me.getPosition());
			} else {
				this.me.setBodyDir(Vektor.normalize(headAngle
						- this.me.getNeckAngle()));
			}

			this.me.setPosition(newPos);
			if (this.world.collided()) {
				this.mclFilter.reset(newPos);
			}

			this.me.setCycle(vInfo.getCycle());
			this.me.setBodyCycle(this.world.getBodyCycle());
		} // end if
	}

	/**
	 * <code>updateAural</code> updates the player <code>me</code> from the
	 * AuralInfo <code>aInfo</code>
	 * 
	 * @param aInfo
	 */
	public void update(RefereeInfo info) {

    //reset recovery after half time
		if ((info).getRefereeMessageType() == REFEREE_MESSAGE.HALF_TIME) {
			this.me.setRecovery(this.sConf.RECOVER_MAX);
		}
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayModeInfo)
	 */
	public void update(PlayModeInfo info) {
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see robocup.component.worldmodel.IModel#update(robocup.component.infotypes.PlayerSayInfo)
	 */
	public void update(PlayerSayInfo info) {
		if (info == null)
			return;
		this.me.setLastHeared(this.world.getBodyCycle(), info.getNumber(),
				world.getMessageFactory().getMessageTypes().get(0));
	}

	/**
	 * the difference between the current and (the last position + last speed)
	 * 
	 * @return
	 */
	public Vektor getSelfPositionEstimationError() {

		return this.me.getPosition().sub(this.tmpMe.getPosition());
	}

	/**
	 * <code>approxMyPos</code> calculates player's position from 2 FlagInfo
	 * objects.
	 * 
	 * @param f1 -
	 *            FlagInfo 1
	 * @param f2 -
	 *            FlagInfo 2
	 * @return Vektor - player's position
	 */
	private final PlayerState approxMyPos(FlagInfo f1, FlagInfo f2) {

		try {
			if (f1.getId() == -1 || f2.getId() == -1) {
				return null;
			}
			final Vektor p1 = this.sConf.getFlag(f1.getId()).getPosition();
			final Vektor p2 = this.sConf.getFlag(f2.getId()).getPosition();
			final double pp1 = f1.getDistance();
			final double pp2 = f2.getDistance();

			Vektor newPos = getNewPosition(p1, pp1, Math.toRadians(f1
					.getDirection()), p2, pp2, Math
					.toRadians(f2.getDirection()));

			Vektor temp = p2.sub(newPos);
			temp.rotate(-f2.getDirection());
			double newHeadAngle = temp.getAngle();
			temp = p1.sub(newPos);
			temp.rotate(-f1.getDirection());
			newHeadAngle += temp.getAngle();
			newHeadAngle /= 2;

			if (Double.isNaN(newHeadAngle)) {
				newHeadAngle = this.me.getHeadDir();
			}

			return new PlayerState(newPos, newHeadAngle);
		} // end try
		catch (NullPointerException npe) {

		  return null;
		} // end catch
	}

	/**
	 * <code>getHAFromLine</code> returns the HeadAngle of the player looking
	 * at the line <code>lineInfo</code> & being at <code>pos</code>
	 * 
	 * @param lineInfo -
	 *            line the player stares at
	 * @param pos -
	 *            position of the player
	 * @return double - headangle of the player
	 */
	private double getHAFromLine(LineInfo lineInfo, Vektor pos) {

		final double lineDir = lineInfo.getDirection();
		// horizontal line
		if (isHorizontal(lineInfo)) {
			final double lineY = this.sConf.getLine(lineInfo.getId())
					.getPoint1().y;
			final boolean overLine = (lineY < pos.y);
			// positive lineAngle
			if (lineDir > 0) {
				return (overLine) ? -90 - lineDir : 90 - lineDir;
			} // end if

			// negative lineAngle
			return (overLine) ? 90 - lineDir : -90 - lineDir;
		} // end if

		// Vertical line
		final double lineX = this.sConf.getLine(lineInfo.getId()).getPoint1().x;
		final boolean rightOfLine = (lineX < pos.x);
		// positive lineAngle
		if (lineDir > 0) {
			return (rightOfLine) ? -lineDir : 180 - lineDir;
		} // end if

		// negative lineAngle
		return (rightOfLine) ? -180 - lineDir : -lineDir;
	}

	/**
	 * <code>getNewPosition</code> returns the position of the player. This
	 * method implements the position calculation algorithm from the MT
	 * (6.4.2.1, Method 2). There is a slight modification however.
	 * <code>h</code> is calculated using Pitagoras instead of cosinus, which
	 * seems to be better (test with trainer when available!). This method works
	 * well, but the algorithm is not too efficient. This is not critical, since
	 * we only use it for a preliminary approximation most of the time.
	 * 
	 * @param f -
	 *            position of the first Flag
	 * @param fr -
	 *            distance from player to the first Flag
	 * @param fAngle -
	 *            angle between of the first Flag (rel. to HA)
	 * @param g -
	 *            position of the second Flag
	 * @param gr -
	 *            distance from player to the second Flag
	 * @param gAngle -
	 *            angle between of the second Flag (rel. to HA)
	 * @return Vektor - position of the player
	 */
	private Vektor getNewPosition(Vektor f, double fr, double fAngle, Vektor g,
			double gr, double gAngle) {

		final double fx = f.x, fy = f.y, gx = g.x, gy = g.y;
		final double deltaX = (gx - fx), deltaY = (gy - fy);
		if (deltaX == 0 && deltaY == 0) {
			return null;
		} // end if
		final double d = Math.sqrt((deltaX) * (deltaX) + (deltaY) * (deltaY));
		final double a = (fr * fr - gr * gr + d * d) / (2 * d);
		// TODO Smarqoo, Feb 14, 2005, 1:44:13 AM, Test cosinus with trainer
		// less Vektor creation (rw)
		final double h = Math.sqrt(Math.abs(fr * fr - a * a));
		// final double h = Math.cos(alpha) * fr;

		Vektor fa = g.sub(f);
		fa.setLength(a);
		boolean fLeft = (fAngle < gAngle) ? true : false;
		fLeft = (a < 0) ? !fLeft : fLeft;
		double rotate = (fLeft) ? fa.getAngle() + 90 : fa.getAngle() - 90;
		Vektor toNewPos = new Vektor(h, rotate);

		return f.add(fa).add(toNewPos);
	}

	/**
	 * <code>averagePosStdDev</code> calculates the average position from a
	 * Vector of Vektors using standard deviation weights
	 * 
	 * @param positions -
	 *            Vector with positions (Vektors)
	 * @return Vektor - average position
	 */
	private final Vektor averagePosStdDev(Vector<Vektor> positions) {

		if (positions == null || positions.size() == 0) {
			return null;
		}
		Vektor av = new Vektor(0, 0);
		int count = 0;
		Vektor[] posArray = new Vektor[positions.size()];

		for (Vektor v : positions) {
			if (v != null) {
				// rw was macht denn dieses if? koennen x und y NaN sein??
				if (!(Double.isNaN(v.x) || Double.isNaN(v.y))) {
					posArray[count] = v;
					av.addToThis(v);
					count++;
				}
			}
		}

		if (count == 0) {
			return null;
		}

		double[] xWeights = new double[count];
		double[] yWeights = new double[count];
		// means
		final double xMean = av.x / count;
		final double yMean = av.y / count;

		// rw added +1 for avoiding div 0
		for (int i = 0; i < count; i++) {
			// weight (1/sigma^2)
			xWeights[i] = 1 / ((posArray[i].x - xMean)
					* (posArray[i].x - xMean) + 1);
			yWeights[i] = 1 / ((posArray[i].y - yMean)
					* (posArray[i].y - yMean) + 1);
		}

		double xWeightSum = xWeights[0];
		double yWeightSum = yWeights[0];
		double weightedXSum = xWeights[0] * posArray[0].x;
		double weightedYSum = yWeights[0] * posArray[0].y;
		for (int i = 1; i < count; i++) {
			weightedXSum += xWeights[i] * posArray[i].x;
			weightedYSum += yWeights[i] * posArray[i].y;
			xWeightSum += xWeights[i];
			yWeightSum += yWeights[i];
		}

		// return our beautiful result ;)
		return new Vektor(Vektor.XY, weightedXSum / xWeightSum, weightedYSum
				/ yWeightSum);
	}

	/**
	 * <code>averageHAStdDev<code> calculates the average HeadAngle from a 
	 * Vector of angles using standard deviation weights.
	 * 
	 * @param headDirections    - Vector with various head angles (double)
	 * @return double           - average head angle
	 */
	private double averageHAStdDev(Vector<Double> headDirections) {

		if (headDirections == null || headDirections.size() == 0) {
			// rw bitte dann auch abfangen!! (und mich nicht nach NaNs suchen
			// lassen)
			// sollten werte nicht berechnet werden kï¿½nnen (was an sich
			// schon komisch wï¿½re)
			// so nehmt bitte die geschï¿½tzten werte aus dem prophet
			return Double.NaN;
		} // end if

		int count = 0;
		double mean = .0;
		for (double element : headDirections) {
			if (!Double.isNaN(element)) {
				mean += Vektor.normalize(element);
				count++;
			}
		}

		mean /= count;
		double[] weights = new double[count];
		int j = 0;
		for (double data : headDirections) {
			// weight (1/sigma^2)
			// rw added +1 avoiding div 0
			weights[j] = 1 / ((data - mean) * (data - mean) + 1);
			j++;
		}

		double weightSum = .01;
		double weightedSum = .01;
		int i = 0;
		for (double weight : weights) {
			weightSum += weight;
			weightedSum += weight * headDirections.get(i);
			i++;
		} // end for
		// return our beautiful result ;)
		return (weightedSum / weightSum);
	}

	/**
	 * <code>isHorizontal</code> returns true if the LineInfo <code>l</code>
	 * represents a horizontal line DONE: 100%
	 * 
	 * @param l -
	 *            LineInfo to analyze
	 * @return boolean - true if LineInfo represents a horizontal line, false
	 *         otherwise
	 */
	private final boolean isHorizontal(LineInfo l) {

		final Line line = this.sConf.getLine(l.getId());
		return line != null && line.getPoint1().y == line.getPoint2().y;
	}

	/**
	 * Tells if an object should be seen.
	 * 
	 * @param pos
	 * @return true if object in viewCone or visibleDistance
	 */
	public boolean shouldBeSeen(StaticObject o) {

		if (o instanceof Ball) {
			return Math.abs(me.getAngleRel(o.getPosition(), RELATIVE.TO_HEAD)) < (this.world
					.getViewWidth() - 15.0) / 2
					|| this.me.getPosition().getDistance(o.getPosition()) < this.sConf.VISIBLE_DISTANCE - 0.5;
		} else {
			return Math.abs(me.getAngleRel(o.getPosition(), RELATIVE.TO_HEAD)) < (this.world
					.getViewWidth()) / 2
					|| this.me.getPosition().getDistance(o.getPosition()) < this.sConf.VISIBLE_DISTANCE;
		}
	}

	/**
	 * <code>getBodyDir</code>
	 * 
	 * @return double
	 * @see robocup.component.worldobjects.Player#getBodyDir()
	 */
	public final double getBodyDir() {
		if (this.me == null)
			return 0.0;

		return this.me.getBodyDir();
	}

	/**
	 * <code>getHeadDir</code>
	 * 
	 * @return double
	 * @see robocup.component.worldobjects.Player#getHeadDir()
	 */
	public final double getHeadDir() {

		return this.me.getHeadDir();
	}

	/**
	 * <code>getNeckAngle</code>
	 * 
	 * @return double
	 * @see robocup.component.worldobjects.Player#getNeckAngle()
	 */
	public final double getNeckAngle() {

		return this.me.getNeckAngle();
	}

	/**
	 * <code>getNumber</code>
	 * 
	 * @return int
	 * @see robocup.component.worldobjects.Player#getNumber()
	 */
	public final int getNumber() {

		return this.me.getNumber();
	}

	/**
	 * <code>getSpeed</code>
	 * 
	 * @return The player's velocity. Note that if you perform operations on
	 *         this Vektor that change it, you need to clone it first!
	 */
	public Vektor getSpeed() {

		return this.me.getSpeed();
	}

	/**
	 * <code>getPosition</code>
	 * 
	 * @return Returns the player's position. Note that if you perform
	 *         operations on this Vektor that change it, you need to clone it
	 *         first!
	 */
	public Vektor getPosition() {

		return this.me.getPosition();
	}

	/**
	 * @return the view width in degree.
	 */
	public final double getViewWidth() {

		return this.viewWidth;
	}

	/**
	 * @return Returns the me as clone.
	 */
	public Player getPlayerClone() {

		return this.me.cloned();
	}

	/**
	 * @return Returns the lastMe.
	 */
	public Player getLastMe() {

		return this.lastMe;
	}

	/**
	 * @return Returns the predictedMe.
	 */
	public Player getPredictedMe() {

		return this.predictedMe;
	}

  /**
   * resets the particle filters after we collided
   */
	public void resetFilterAfterCollisionWithBall() {
		this.mclFilter.reset(this.me.getPosition());
    this.particleFilter.resetPosition(this.me.getPosition());
	}

}
